/***************************************************************************************************************************
* px4_pose_controller.cpp
*
* Author: Wr
*
* Update Time: 2021.3.26
*
* Fuction:  Px4控制 
*         1. 从上层节点订阅/px4/control_command话题（类型为Px4ControlCommand.msg），接收来自上层的控制指令
                该指令并不是进行px4控制的指令，而是包含了控制模式等更多指令信息的上层指令，需要借助于位置控制器等工具作用于px4
*         2. 从px4_pose_estimator.cpp节点订阅无人机的状态信息（Px4State.msg）
*     
           3. 调用位置环控制算法，计算加速度控制量，并转换为期望角度。（可选择PID,cascade_PID, PID等位置控制算法）
*         4. 通过command_to_mavros.h将计算出来的控制指令发送至飞控
*     
***************************************************************************************************************************/

#include <ros/ros.h>

//自定义的工具
#include "state_from_mavros.h"
#include "command_to_mavros.h"

#include "px4_control_utils.h"
#include "Position_Controller/pose_controller_PID.h"

#include "px4_msgs/Px4State.h"
#include "px4_msgs/Px4ControlCommand.h"
#include "px4_msgs/Px4ControlOutput.h"
#include "px4_msgs/Px4AttitudeReference.h"


using namespace std;

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>变量声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
float cur_time;                                     //程序运行时间
int controller_number;                             //选择控制器编号         可方便后续设计多种控制器验证不同控制效果
float Takeoff_height;                        //起飞高度
float Disarm_height;                        //自动上锁高度  超过此高度自动上锁
float Land_speed;                             //降落速度

int x = 0;

//Geigraphical fence 地理围栏
Eigen::Vector2f geo_fence_x;
Eigen::Vector2f geo_fence_y;
Eigen::Vector2f geo_fence_z;


px4_msgs::Px4State _Px4State;      //Px4状态量
px4_msgs::Px4ControlCommand Command_Now;   //接收到的当前控制指令(包含控制模式，位置期望等)
px4_msgs::Px4ControlCommand Command_Last;    //上一条控制指令

px4_msgs::Px4ControlOutput _ControlOutput;                   //期望推力及油门输出
px4_msgs::Px4AttitudeReference  _AttitudeReference;           //位置控制器输出，即姿态环参考量

Eigen::Vector3d _PosReference;  //mission 模块下发的期望位置
Eigen::Vector3d _VelReference;  //mission 模块下发的期望速度

geometry_msgs::PoseStamped ref_pose_rviz;   //

float dt = 0;

ros::Publisher att_ref_pub;   //发布期望姿态给command_to_mavros接口使用      利用了位置速度加速度数据，理想的控制方式
ros::Publisher ref_pose_pub;  //发布期望位姿  rviz显示用
// Todo :  ros::Publisher vel_ref_pub   //发布期望速度      利用位置速度数据，检验不同控制方式的控制效果

//ros::Publisher message_pub;
//ros::Publisher log_message_pub;

Eigen::Vector3d throttle_sp;  //期望油门

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>函数声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int check_failsafe();     //检查是否超出设置的地理围栏，是则执行降落命令
void printf_param();    //打印参数
void Body_to_ENU();    //机体坐标系转换到ENU坐标系
//void add_disturbance();
geometry_msgs::PoseStamped get_ref_pose_rviz (const px4_msgs::Px4ControlCommand& cmd, const px4_msgs::Px4AttitudeReference& att_ref);

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回调函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void Command_cb(const px4_msgs::Px4ControlCommand::ConstPtr &msg)
{
    // CommandID必须递增才会被记录,确保接收到的控制指令时间上是顺序的
    if(msg->Command_ID > Command_Now.Command_ID)
    {
        Command_Now = *msg;  //将刚接收到的指令赋值给当前指令
    }
    else 
    {
        cout << "Command_ID:  " << msg->Command_ID 
                  << "  Command_Now_ID: " <<  Command_Now.Command_ID<<endl;
        ROS_INFO("Command_ID is wrong.   Please check Mission module ");
    }
    // 无人机一旦接受到Disarm指令，则执行降落，后续接收到的指令不再更新，保证顺利降落
    if(Command_Last.Mode == px4_msgs::Px4ControlCommand::Disarm)
    {
        Command_Now = Command_Last;     //即屏蔽后续指令
    }
}

void px4_state_cb(const px4_msgs::Px4State::ConstPtr &msg)
{
    _Px4State = *msg;
    _Px4State.time_from_start = cur_time;
}

void timerCallback(const ros::TimerEvent& e)
{
    ROS_INFO("Program is running.");
}


int main(int argc, char** argv)
{
    ros::init(argc, argv, "px4_pose_controller");
    ros::NodeHandle nh("~");
 
    //【订阅】  上层任务指令
    //   控制指令类型为 Px4ControlCommand.msg
    ros::Subscriber Command_sub = nh.subscribe<px4_msgs::Px4ControlCommand>("/px4/control_command", 10, Command_cb);
    
    //【订阅】  无人机当前状态
    //   本话题由px4_pose_estimator.cpp 发布
    ros::Subscriber px4_state_sub = nh.subscribe<px4_msgs::Px4State>("/px4/state", 10, px4_state_cb);

    //【发布】 位置控制器的输出量:期望姿态     利用位置速度加速度数据，得到期望推力与油门，经过转换后得到期望姿态与油门
    att_ref_pub = nh.advertise<px4_msgs::Px4AttitudeReference>("/px4/control/attitude_reference", 10);  
    
    //Todo：【发布】期望速度    利用位置速度数据，得到期望速度
    // vel_ref_pub =nh.advertise<自定义消息类型>("/px4/control/velocity_reference", 10);

    //【发布】期望位姿 RVIZ显示用
    ref_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/px4/control/ref_pose_rviz", 10);    

    // 10秒定时打印，以确保程序在正确运行
    ros::Timer timer = nh.createTimer(ros::Duration(10.0), timerCallback);

    // 参数读取
    nh.param<int>("controller_number", controller_number, 1);
    nh.param<float>("Takeoff_height", Takeoff_height, 0.5);
    nh.param<float>("Disarm_height", Disarm_height, 3);
    nh.param<float>("Land_speed", Land_speed, 0.2);
    
    nh.param<float>("geo_fence/x_min", geo_fence_x[0], -100.0);
    nh.param<float>("geo_fence/x_max", geo_fence_x[1], 100.0);
    nh.param<float>("geo_fence/y_min", geo_fence_y[0], -100.0);
    nh.param<float>("geo_fence/y_max", geo_fence_y[1], 100.0);
    nh.param<float>("geo_fence/z_min", geo_fence_z[0], -100.0);
    nh.param<float>("geo_fence/z_max", geo_fence_z[1], 100.0);

    // 位置控制一般选取为50Hz，主要取决于位置状态的更新频率
    ros::Rate rate(50.0);

    float time_trajectory = 0.0;

    // 用于与mavros通讯的类，通过mavros发送控制指令至飞控【本程序->mavros->飞控】
    command_to_mavros px4_command_to_mavros;

    // 位置控制器声明   串级PID   此控制器追踪位置
   // pose_controller_cascade_PID pose_controller_cascade_pid;
    //Todo：可以设置自定义位置环控制算法  验证不同控制效果
    // PID控制器  可追踪位置和速度
    pose_controller_PID pose_controller_pid;



    printf_param();

    if(controller_number == 0)
    {
        pose_controller_pid.printf_param();        
    }
    if(controller_number == 1)
    {
       // pose_controller_cascade_pid.printf_param();        
    }

    // 先读取一些飞控的数据
    for(int i=0;i<100;i++)
    {
        ros::spinOnce();
        rate.sleep();
    }

    // Set the takeoff position  先设置为无人机当前位置(执行程序后，起飞前)
    Eigen::Vector3d Takeoff_position;
    Takeoff_position[0] = _Px4State.position[0];
    Takeoff_position[1] = _Px4State.position[1];
    Takeoff_position[2] = _Px4State.position[2];

    // 初始化指令
    // 默认设置：Idle模式 电机怠速旋转 等待来自上层应用程序的控制指令
    Command_Now.Mode                                = px4_msgs::Px4ControlCommand::Idle;
    Command_Now.Command_ID                          = 0;
    Command_Now.Px4_Reference_State.Move_mode          = px4_msgs::Px4PositionReference::XYZ_POS;  //初始化为XYZ位置追踪模式
    Command_Now.Px4_Reference_State.Move_frame          = px4_msgs::Px4PositionReference::ENU_FRAME;
    Command_Now.Px4_Reference_State.position_ref[0]     = 0;
    Command_Now.Px4_Reference_State.position_ref[1]     = 0;
    Command_Now.Px4_Reference_State.position_ref[2]     = 0;
    Command_Now.Px4_Reference_State.velocity_ref[0]     = 0;
    Command_Now.Px4_Reference_State.velocity_ref[1]     = 0;
    Command_Now.Px4_Reference_State.velocity_ref[2]     = 0;
    Command_Now.Px4_Reference_State.acceleration_ref[0] = 0;
    Command_Now.Px4_Reference_State.acceleration_ref[1] = 0;
    Command_Now.Px4_Reference_State.acceleration_ref[2] = 0;
    Command_Now.Px4_Reference_State.yaw_ref             = 0;

    // 记录启控时间
    ros::Time begin_time = ros::Time::now();
    float last_time = px4_control_utils::get_time_in_sec(begin_time);


    //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主  循  环<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    while(ros::ok())
    {
        // 当前时间
        cur_time = px4_control_utils::get_time_in_sec(begin_time);
        dt = cur_time  - last_time;
        dt = constrain_function2(dt, 0.01, 0.03);
        last_time = cur_time;

        //执行回调函数
        ros::spinOnce();
        
        //检查地理围栏，飞机超出地理围栏将执行降落
        if(check_failsafe() == 1)
        {
            Command_Now.Mode = px4_msgs::Px4ControlCommand::Land;
        }
       
        //主循环中一直检查上层应用的指令，根据指令模式执行不同操作
        switch(Command_Now.Mode)
        {
        //Idle模式   怠速旋转，此时可以切入offboard模式，但不会起飞
        case px4_msgs::Px4ControlCommand::Idle:
            px4_command_to_mavros.idle();   //调用command_to_mavros类中的函数Idle()

            //Todo: 设定yaw_ref=999时，切换offboard模式，并解锁
            if(Command_Now.Px4_Reference_State.yaw_ref == 999)
            {
                while(_Px4State.mode != "OFFBOARD")    //这时切换OFFBOARD模式，发送给飞控的期望位姿是什么？
                {
                    px4_command_to_mavros.mode_cmd.request.custom_mode = "OFFBOARD";
                    px4_command_to_mavros.set_mode_client.call(px4_command_to_mavros.mode_cmd);

                    ROS_INFO("Now  Setting to OFFBOARD Mode.......");           
                    //执行回调函数
                    ros::spinOnce();
                    ros::Duration(0.5).sleep();
                }
                while(!_Px4State.armed)
                {
                    px4_command_to_mavros.arm_cmd.request.value = true;
                    px4_command_to_mavros.arming_client.call(px4_command_to_mavros.arm_cmd);
                    ROS_INFO("Now  Arming......."); 
                    //执行回调函数
                    ros::spinOnce();
                    ros::Duration(0.5).sleep();
                }
            }

            break;
        
        // 【Takeoff】 从摆放初始位置原地起飞至指定高度，偏航角也保持当前角度
        case px4_msgs::Px4ControlCommand::Takeoff:
            if (Command_Last.Mode != px4_msgs::Px4ControlCommand::Takeoff)
            {
                Command_Now.Px4_Reference_State.Move_mode       = px4_msgs::Px4PositionReference::XYZ_POS;
                Command_Now.Px4_Reference_State.Move_frame       = px4_msgs::Px4PositionReference::ENU_FRAME;
                //设置起飞x，y位置不变，偏航角不变，z方向为设定的起飞高度
                Command_Now.Px4_Reference_State.position_ref[0] = Takeoff_position[0];
                Command_Now.Px4_Reference_State.position_ref[1] = Takeoff_position[1];
                Command_Now.Px4_Reference_State.position_ref[2] = Takeoff_position[2] + Takeoff_height;  
                //这里欧拉角由自定义函数quaternion_to_euler()计算得到，顺序为RPY
                Command_Now.Px4_Reference_State.yaw_ref         = _Px4State.attitude[2];
                //cout << 666 << endl;
                //cout << Command_Now.Px4_Reference_State.position_ref[2] << endl;
            }
            break;
        // 【Hold】 保持当前位置悬停
        case px4_msgs::Px4ControlCommand::Hold:
            if (Command_Last.Mode != px4_msgs::Px4ControlCommand::Hold)
            {
                Command_Now.Px4_Reference_State.Move_mode       = px4_msgs::Px4PositionReference::XYZ_POS;
                Command_Now.Px4_Reference_State.Move_frame       = px4_msgs::Px4PositionReference::ENU_FRAME;
                Command_Now.Px4_Reference_State.position_ref[0] = _Px4State.position[0];
                Command_Now.Px4_Reference_State.position_ref[1] = _Px4State.position[1];
                Command_Now.Px4_Reference_State.position_ref[2] = _Px4State.position[2];
                Command_Now.Px4_Reference_State.yaw_ref         = _Px4State.attitude[2];        //rad
            }
            break;
            //Land】当前位置原地降落，降落后会自动上锁，且切换为mannual手动模式
        case px4_msgs::Px4ControlCommand::Land:
            if(Command_Last.Mode != px4_msgs::Px4ControlCommand::Land)
            {
                Command_Now.Px4_Reference_State.Move_mode      = px4_msgs::Px4PositionReference::XY_POS_Z_VEL;
                Command_Now.Px4_Reference_State.Move_frame      = px4_msgs::Px4PositionReference::ENU_FRAME;
                Command_Now.Px4_Reference_State.position_ref[0] = _Px4State.position[0];
                Command_Now.Px4_Reference_State.position_ref[1] = _Px4State.position[1];
                Command_Now.Px4_Reference_State.velocity_ref[2] = - Land_speed; //Land_speed
                Command_Now.Px4_Reference_State.yaw_ref         = _Px4State.attitude[2]; //rad
            }
            //如果距离起飞高度小于10厘米，则直接上锁并切换为手动模式；
            if(abs(_Px4State.position[2] - Takeoff_position[2]) < Disarm_height)
            {
                if(_Px4State.mode == "OFFBOARD" )
                {
                    //此处切换回manual模式是因为:PX4默认在offboard模式且有控制的情况下没法上锁
                    px4_command_to_mavros.mode_cmd.request.custom_mode = "MANUAL";
                    px4_command_to_mavros.set_mode_client.call(px4_command_to_mavros.mode_cmd);
                    ROS_INFO("Command : Land   switch to MANUAL flight mode.........");           
                }
                if(_Px4State.armed)
                {
                    px4_command_to_mavros.arm_cmd.request.value = false;
                    px4_command_to_mavros.arming_client.call(px4_command_to_mavros.arm_cmd);
                }
            }
            if(_Px4State.landed)   //如果已经在Land模式下了，修改指令为怠速模式？
            {
                Command_Now.Mode = px4_msgs::Px4ControlCommand::Idle;
            }
            break;
        //【Move】 在ENU系下移动      注：只有PID算法中才有追踪速度的选项，其他控制只能追踪位置
        case px4_msgs::Px4ControlCommand::Move:
            //对于机体系NED的指令,需要转换成ENU坐标系执行,且同一指令ID号内,只执行一次.
            if(Command_Now.Px4_Reference_State.Move_frame != px4_msgs::Px4PositionReference::ENU_FRAME && Command_Now.Command_ID  >  Command_Last.Command_ID )
            {
                Body_to_ENU();
            }
            break;
        //【Disarm】 上锁
        case px4_msgs::Px4ControlCommand::Disarm:
            ROS_INFO("Command : Disarm   switch to MANUAL flight mode.........");   
            if(_Px4State.mode == "OFFBOARD")
            {
                px4_command_to_mavros.mode_cmd.request.custom_mode = "MANUAL";
                px4_command_to_mavros.set_mode_client.call(px4_command_to_mavros.mode_cmd);
            }
            if(_Px4State.armed)
            {
                px4_command_to_mavros.arm_cmd.request.value = false;
                px4_command_to_mavros.arming_client.call(px4_command_to_mavros.arm_cmd);
            }
            break;
        // 【User_Mode1】 暂空。可进行自定义
        case px4_msgs::Px4ControlCommand::User_Mode1:
            
            break;

        // 【User_Mode2】 暂空。可进行自定义
        case px4_msgs::Px4ControlCommand::User_Mode2:
            
            break;
        default:
            cout << "没进去" <<endl;

        }//更新Command指令结束
        //执行控制指令
        if(Command_Now.Mode != px4_msgs::Px4ControlCommand::Idle)
        {
            _PosReference[0] = Command_Now.Px4_Reference_State.position_ref[0];
            _PosReference[1] = Command_Now.Px4_Reference_State.position_ref[1];
            _PosReference[2] = Command_Now.Px4_Reference_State.position_ref[2];
            _VelReference[0] = Command_Now.Px4_Reference_State.velocity_ref[0] * 0.3;
            _VelReference[1] = Command_Now.Px4_Reference_State.velocity_ref[1] * 0.3;
            _VelReference[2] = Command_Now.Px4_Reference_State.velocity_ref[2] * 0.3;

            px4_command_to_mavros.send_pos_setpoint(_PosReference, Command_Now.Px4_Reference_State.yaw_ref);     
            if(Command_Now.Mode == px4_msgs::Px4ControlCommand::Move)
            {
                px4_command_to_mavros.send_vel_unstamped_setpoint(_VelReference);
            }
                
        }

    /*
        //执行控制指令
        if(Command_Now.Mode != px4_msgs::Px4ControlCommand::Idle)
        {
            //选择控制器
            if(controller_number == 0)   //Normal PID
            { 
                //调用PID控制器，返回期望推力
                _ControlOutput = pose_controller_pid.pose_controller(_Px4State, Command_Now.Px4_Reference_State, dt);
            }
            else if(controller_number == 1)  //串级PID
            {
                //轨迹追踪控制,直接改为PID控制器
                if(Command_Now.Px4_Reference_State.Move_mode != px4_msgs::Px4PositionReference::TRAJECTORY)
                {
                    ROS_INFO("pose_controller_cascade_pid is not exist");   
                    //_ControlOutput = pose_controller_cascade_pid.pos_controller(_Px4State, Command_Now.Px4_Reference_State, dt);                    
                }
                else
                {
                    _ControlOutput = pose_controller_pid.pose_controller(_Px4State, Command_Now.Px4_Reference_State, dt);
                    ROS_INFO("Command Mode : TRAJECTORY   Controller switch ：pose_controller_pid");   
                }
            }
        }          //经过控制器计算得到了期望油门

        //期望油门
        throttle_sp[0] = _ControlOutput.Throttle[0];
        throttle_sp[1] = _ControlOutput.Throttle[1];
        throttle_sp[2] = _ControlOutput.Throttle[2];

 
        /* Fuction：期望油门转期望姿态
        *input： PID控制器输出的期望油门，Command指令期望偏航
        *output：期望姿态
        
        _AttitudeReference = px4_control_utils::ThrottleToAttitude(throttle_sp, Command_Now.Px4_Reference_State.yaw_ref);
    */
        //发送解算得到的期望姿态角至Px4   
        //这是本节点目前采用的控制模式
        //px4_command_to_mavros.send_attitude_setpoint(_AttitudeReference); 

        //Todo：发位置至Px4
        //cout << 777 << endl;
        //cout << "777 :  "<<  Command_Now.Px4_Reference_State.position_ref[2] << endl;
        //Eigen::Vector3d pos_sp(Command_Now.Px4_Reference_State.position_ref[0], Command_Now.Px4_Reference_State.position_ref[1], Command_Now.Px4_Reference_State.position_ref[2] );
        //Eigen::Vector3d pos_sp(0, 0, 0.5 );
        //px4_command_to_mavros.send_pos_setpoint(pos_sp, Command_Now.Px4_Reference_State.yaw_ref); 


        //发布期望姿态  给其他节点？
        //att_ref_pub.publish(_AttitudeReference);

        //发布用于RVIZ显示的期望位姿   
        //ref_pose_rviz = get_ref_pose_rviz(Command_Now, _AttitudeReference);   
        //ref_pose_pub.publish(ref_pose_rviz);

        Command_Last = Command_Now;
        rate.sleep();
        
    }
    
    return 0;
}

void printf_param()
{
    cout <<">>>>>>>>>>>>>>>>>>>>>>>> px4_pose_controller Parameter <<<<<<<<<<<<<<<<<<<<<<" <<endl;
    cout << "controller_number: "<< controller_number <<endl;
    cout << "Takeoff_height   : "<< Takeoff_height<<" [m] "<<endl;
    cout << "Disarm_height    : "<< Disarm_height <<" [m] "<<endl;
    cout << "Land_speed       : "<< Land_speed <<" [m/s] "<<endl;
    cout << "geo_fence_x : "<< geo_fence_x[0] << " [m]  to  "<<geo_fence_x[1] << " [m]"<< endl;
    cout << "geo_fence_y : "<< geo_fence_y[0] << " [m]  to  "<<geo_fence_y[1] << " [m]"<< endl;
    cout << "geo_fence_z : "<< geo_fence_z[0] << " [m]  to  "<<geo_fence_z[1] << " [m]"<< endl;

}
//检查是否超出地理围栏，超出：返回1   
int check_failsafe()
{
    if (_Px4State.position[0] < geo_fence_x[0] || _Px4State.position[0] > geo_fence_x[1] ||
         _Px4State.position[1] < geo_fence_y[0] || _Px4State.position[1] > geo_fence_y[1] ||
         _Px4State.position[2] < geo_fence_z[0] || _Px4State.position[2] > geo_fence_z[1])
    {
        ROS_INFO("Out of the geo fence, the drone is landing........");
        return 1;
    }
    else{
        return 0;
    }
}

//【Body_to_ENU】 机体系移动

void Body_to_ENU()
{
    if( Command_Now.Px4_Reference_State.Move_mode  & 0b10 )
    {
        //xy velocity mode
        float d_vel_body[2] = {Command_Now.Px4_Reference_State.velocity_ref[0], Command_Now.Px4_Reference_State.velocity_ref[1]};         //the desired xy velocity in Body Frame
        float d_vel_enu[2];                                                           //the desired xy velocity in NED Frame

        //根据无人机当前偏航角进行坐标系转换
        px4_control_utils::rotation_yaw(_Px4State.attitude[2], d_vel_body, d_vel_enu);
        Command_Now.Px4_Reference_State.position_ref[0] = 0;
        Command_Now.Px4_Reference_State.position_ref[1] = 0;
        Command_Now.Px4_Reference_State.velocity_ref[0] = d_vel_enu[0];
        Command_Now.Px4_Reference_State.velocity_ref[1] = d_vel_enu[1];
    }
    else
    {   
        //xy position mode
        float d_pos_body[2] = {Command_Now.Px4_Reference_State.position_ref[0], Command_Now.Px4_Reference_State.position_ref[1]};         //the desired xy position in Body Frame
        float d_pos_enu[2];                                                           //the desired xy position in enu Frame (The origin point is the drone)
        px4_control_utils::rotation_yaw(_Px4State.attitude[2], d_pos_body, d_pos_enu);

        Command_Now.Px4_Reference_State.position_ref[0] = _Px4State.position[0] + d_pos_enu[0];
        Command_Now.Px4_Reference_State.position_ref[1] = _Px4State.position[1] + d_pos_enu[1];
        Command_Now.Px4_Reference_State.velocity_ref[0] = 0;
        Command_Now.Px4_Reference_State.velocity_ref[1] = 0;
    }

    if(Command_Now.Px4_Reference_State.Move_frame == px4_msgs::Px4PositionReference::MIX_FRAME)
    {
        Command_Now.Px4_Reference_State.position_ref[2] = Command_Now.Px4_Reference_State.position_ref[2];
        //Command_Now.Reference_State.yaw_ref = Command_Now.Reference_State.yaw_ref;
    }else
    {
        if( Command_Now.Px4_Reference_State.Move_mode  & 0b01 )
        {
            //z velocity mode
            Command_Now.Px4_Reference_State.position_ref[2] = 0;
            Command_Now.Px4_Reference_State.velocity_ref[2] = Command_Now.Px4_Reference_State.velocity_ref[2];
        }
        else
        {   
            //z posiiton mode
            Command_Now.Px4_Reference_State.position_ref[2] = _Px4State.position[2] + Command_Now.Px4_Reference_State.position_ref[2];
            Command_Now.Px4_Reference_State.velocity_ref[2] = 0; 
        }
        
    }

    Command_Now.Px4_Reference_State.yaw_ref = _Px4State.attitude[2] + Command_Now.Px4_Reference_State.yaw_ref;
    float d_acc_body[2] = {Command_Now.Px4_Reference_State.acceleration_ref[0], Command_Now.Px4_Reference_State.acceleration_ref[1]};       
    float d_acc_enu[2]; 

    px4_control_utils::rotation_yaw(_Px4State.attitude[2], d_acc_body, d_acc_enu);
    Command_Now.Px4_Reference_State.acceleration_ref[0] = d_acc_enu[0];
    Command_Now.Px4_Reference_State.acceleration_ref[1] = d_acc_enu[1];
    Command_Now.Px4_Reference_State.acceleration_ref[2] = Command_Now.Px4_Reference_State.acceleration_ref[2];


}


geometry_msgs::PoseStamped get_ref_pose_rviz(const px4_msgs::Px4ControlCommand& cmd, const px4_msgs::Px4AttitudeReference& att_ref)
{
    geometry_msgs::PoseStamped ref_pose;

    ref_pose.header.stamp = ros::Time::now();
    ref_pose.header.frame_id = "world";

    if(cmd.Mode == px4_msgs::Px4ControlCommand::Idle)
    {
        ref_pose.pose.position.x = _Px4State.position[0];
        ref_pose.pose.position.y = _Px4State.position[1];
        ref_pose.pose.position.z = _Px4State.position[2];
        ref_pose.pose.orientation = _Px4State.attitude_q;
    }
    else if(cmd.Mode == px4_msgs::Px4ControlCommand::Takeoff || cmd.Mode == px4_msgs::Px4ControlCommand::Hold)
    {
        ref_pose.pose.position.x = cmd.Px4_Reference_State.position_ref[0];
        ref_pose.pose.position.y = cmd.Px4_Reference_State.position_ref[1];
        ref_pose.pose.position.z = cmd.Px4_Reference_State.position_ref[2];
        ref_pose.pose.orientation = _Px4State.attitude_q;
    }
    else if(cmd.Mode == px4_msgs::Px4ControlCommand::Disarm  || cmd.Mode == px4_msgs::Px4ControlCommand::Land)
    {
        //上锁或降落模式下，参考位置x，y，姿态q不变
        ref_pose.pose.position.x = cmd.Px4_Reference_State.position_ref[0];
        ref_pose.pose.position.y = cmd.Px4_Reference_State.position_ref[1];
        ref_pose.pose.position.z = 0.0;
        ref_pose.pose.orientation = _Px4State.attitude_q;
    }
    else if(cmd.Mode == px4_msgs::Px4ControlCommand::Move)
    {
        //xy速度控制
        if( Command_Now.Px4_Reference_State.Move_mode  & 0b10 )
        {
            ref_pose.pose.position.x = _Px4State.position[0] + cmd.Px4_Reference_State.velocity_ref[0] * dt;
            ref_pose.pose.position.y = _Px4State.position[1] + cmd.Px4_Reference_State.velocity_ref[1] * dt;
        }else
        {
            ref_pose.pose.position.x = cmd.Px4_Reference_State.position_ref[0];
            ref_pose.pose.position.y = cmd.Px4_Reference_State.position_ref[1];
        }

        if(Command_Now.Px4_Reference_State.Move_mode  & 0b01 )
        {
            ref_pose.pose.position.z = _Px4State.position[2] + cmd.Px4_Reference_State.velocity_ref[2] * dt;
        }else
        {
            ref_pose.pose.position.z = cmd.Px4_Reference_State.position_ref[2];
        }

        ref_pose.pose.orientation = att_ref.desired_att_q;
    }
    else
    {
        ref_pose.pose.position.x = 0.0;
        ref_pose.pose.position.y = 0.0;
        ref_pose.pose.position.z = 0.0;
        ref_pose.pose.orientation = _Px4State.attitude_q;
    }

    return ref_pose;
}
